/*!
 * @Author: Tim Elery
 * @Description: This is the header file for the KVH library
 * @File: kvh_cg5100.hpp 
 * @License: GPL
 */

#include <kvh_cg5100/kvh_cg5100.hpp>

namespace KVH
{
    //Constructor
    KVH::KVH()
    {}

    //Destructor
    KVH::~KVH()
    {}

    // Initialize Serial Port
    void KVH::initSerial() 
    {
	    // Initialize variables
	    struct termios terminalInterface;

    	// Open Serial Port
    	serialPort = open(COM_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK);

    	// Throw error if a problem is encountered during serial port initialization
    	if (serialPort < 0)
    	{
    		perror(COM_PORT);
    		exit(-1);
    	}

    	// Zero terminal interface
    	bzero(&terminalInterface, sizeof(terminalInterface));

    	// Configure terminal interface for serial communication
    	terminalInterface.c_cflag = BAUDRATE | CS8 | CLOCAL | CREAD;	// Baud rate, character size, ignore modem status, enable reciever 
    	terminalInterface.c_iflag = IGNPAR;								// Ignore parity
    	terminalInterface.c_oflag = 0;									// Raw output
    	terminalInterface.c_lflag = 0;									// Non-canonical input
    	terminalInterface.c_cc[VTIME] = 0;								// Inter-character timer unused
    	terminalInterface.c_cc[VMIN] = 0;								// No blocking

    	// Configure connection speed
    	cfsetispeed(&terminalInterface, BAUDRATE);
    	cfsetospeed(&terminalInterface, BAUDRATE);

    	// Flush serial port
    	tcflush(serialPort, TCIFLUSH);
    	tcsetattr(serialPort,TCSANOW, &terminalInterface);				// Do it immediately

	    // Return
	    return;
    }

    // Read serial data into packet buffer
    int KVH::readSerial(unsigned char packetBuffer[])
    {
    	// Initialize variables
    	unsigned char serialBuffer[SERIAL_BUFFER_SIZE];
    	int i, n, numBytes;

    	// Initialize number of bytes received counter
    	numBytes = 0;

    	// Ensure at least one full packet is read
    	while (numBytes < IMU_PACKET_SIZE)
    	{
    		// Read data from serial port
    		n = read(serialPort, serialBuffer, SERIAL_BUFFER_SIZE);

    		// For each byte recieved
    		for (i = 0; i < n; i++) 
    		{
    			// Copy the byte to the packet buffer
    			packetBuffer[numBytes] = (unsigned char) serialBuffer[i];

    			// Increment number of bytes received
    			numBytes++;
    		}
    	}
    	// Alert if packet truncation occurs
    	if (numBytes > IMU_PACKET_SIZE)
    		printf("    Truncating Data...\n");

    	// Return number of bytes received
    	return numBytes;
    }

    // Decode IMU Data
    int KVH::decodeIMU(IMUdata *data)
    {
    	// Initialize variables
    	IMUpacket packet;
    	ufloat tempFloat;
    	int pBufferPointer, i;
    	unsigned char packetBuffer[BUFFER_SIZE], checksum;

    	// Read Serial Port Data into packet buffer
    	pBufferPointer = readSerial(packetBuffer) - IMU_PACKET_SIZE;

    	// Calculate checksum
    	checksum = 0x00;
    	for (i = 1; i < IMU_PACKET_SIZE; i++)
    		checksum += packetBuffer[pBufferPointer + i];

    	// If Checksum fails
    	if (checksum != 0x00)
    	{
    		return 0;
    	}
    	else
    	{
    		// Decode the most recent IMU data packet
    		packet.header = packetBuffer[pBufferPointer];
    		packet.data = packetBuffer[pBufferPointer + 4];
    		packet.checksum = packetBuffer[pBufferPointer + 34];

    		// Extract data fields
    		for (i = 0; i < 9; i++)
    		{
    			// Convert to float
    			//tempFloat.byteValue[3] = packetBuffer[pBufferPointer + 4 + (4 * i)];
    			//tempFloat.byteValue[2] = packetBuffer[pBufferPointer + 5 + (4 * i)];
    			//tempFloat.byteValue[1] = packetBuffer[pBufferPointer + 6 + (4 * i)];
    			//tempFloat.byteValue[0] = packetBuffer[pBufferPointer + 7 + (4 * i)];
    			//packet.data[i] = tempFloat.floatValue;
    		}

    		// Update state
			data->xEulerAngle		= packet.data[0];
			data->yEulerAngle		= packet.data[1];
			data->zEulerAngle		= packet.data[2];
    		data->xAccelerometer	= packet.data[3];
    		data->yAccelerometer	= packet.data[4];
    		data->zAccelerometer	= packet.data[5];
			

    		// Return 1 indicating success
    		return 1;
    	}
    }
}
